#include <caros/ur_si_proxy.h>

#include <caros/ur_service_interface.h>
#include <caros/common.h>
#include <caros/common_robwork.h>
#include <caros_common_msgs/EmptySrv.h>
#include <caros_universalrobot/UrServiceServoQ.h>
#include <caros_universalrobot/UrServiceEmpty.h>
#include <caros_universalrobot/UrServiceForceModeUpdate.h>
#include <caros_universalrobot/UrServiceForceModeStart.h>
#include <caros_universalrobot/UrServiceForceModeStop.h>
#include <caros_universalrobot/UrServiceMoveLin.h>
#include <caros_universalrobot/UrServiceMovePtp.h>
#include <caros_universalrobot/UrServiceMovePtpPath.h>
#include <caros_universalrobot/UrServiceMoveVelQ.h>
#include <caros_universalrobot/UrServiceMoveVelT.h>
#include <caros_universalrobot/UrServicePayload.h>
#include <caros_universalrobot/UrServiceSetIO.h>
#include <string>
#include <vector>

#define SPEED_MIN 0.0f
#define SPEED_MAX 1.0f
#define ACCEL_MIN 0.0f
#define ACCEL_MAX 2.0f
#define BLEND_MIN 0.0f
#define BLEND_MAX 2.0f
#define SERVO_LOOKAHEAD_TIME_MIN 0.03f
#define SERVO_LOOKAHEAD_TIME_MAX 0.2f
#define SERVO_GAIN_MIN 100
#define SERVO_GAIN_MAX 2000

using caros::URSIProxy;

URSIProxy::URSIProxy(ros::NodeHandle nodehandle, const std::string& devname, const bool use_persistent_connections)
    : nodehandle_(nodehandle),
      use_persistent_connections_(use_persistent_connections),
      ros_namespace_("/" + devname + "/" + UR_SERVICE_INTERFACE_SUB_NAMESPACE),
      srv_move_lin_(nodehandle_, "move_lin", ros_namespace_, use_persistent_connections_),
      srv_move_ptp_(nodehandle_, "move_ptp", ros_namespace_, use_persistent_connections_),
      srv_move_ptp_path_(nodehandle_, "move_ptp_path", ros_namespace_, use_persistent_connections_),
      srv_move_servo_q_(nodehandle_, "move_servo_q", ros_namespace_, use_persistent_connections_),
      srv_move_servo_stop_(nodehandle_, "move_servo_stop", ros_namespace_, use_persistent_connections_),
      srv_move_force_mode_start_(nodehandle_, "force_mode_start", ros_namespace_, use_persistent_connections_),
      srv_move_force_mode_update_(nodehandle_, "force_mode_update", ros_namespace_, use_persistent_connections_),
      srv_move_force_mode_stop_(nodehandle_, "force_mode_stop", ros_namespace_, use_persistent_connections_),
      srv_move_vel_q_(nodehandle_, "move_vel_q", ros_namespace_, use_persistent_connections_),
      srv_move_vel_t_(nodehandle_, "move_vel_t", ros_namespace_, use_persistent_connections_),
      srv_stop_(nodehandle_, "move_stop", ros_namespace_, use_persistent_connections_)
{
  // states
  sub_robot_state_ = nodehandle_.subscribe(ros_namespace_ + "/robot_state", 1, &URSIProxy::handleRobotState, this);
}

URSIProxy::~URSIProxy()
{
}

bool URSIProxy::moveLin(const rw::math::Transform3D<>& target, const float speed, const float acceleration)
{
  verifyValueIsWithin(speed, SPEED_MIN, SPEED_MAX);
  verifyValueIsWithin(acceleration, ACCEL_MIN, ACCEL_MAX);
  caros_universalrobot::UrServiceMoveLin srv;
  srv.request.target = caros::toRos(target);
  srv.request.speed = caros::toRos(speed);
  srv.request.acceleration = caros::toRos(acceleration);

  srv_move_lin_.call<caros_universalrobot::UrServiceMoveLin>(srv);

  return srv.response.success;
}

bool URSIProxy::movePtp(const rw::math::Q& target, const float speed, const float acceleration)
{
  verifyValueIsWithin(speed, SPEED_MIN, SPEED_MAX);
  verifyValueIsWithin(acceleration, ACCEL_MIN, ACCEL_MAX);
  caros_universalrobot::UrServiceMovePtp srv;
  srv.request.target = caros::toRos(target);
  srv.request.speed = caros::toRos(speed);
  srv.request.acceleration = caros::toRos(acceleration);

  srv_move_ptp_.call<caros_universalrobot::UrServiceMovePtp>(srv);

  return srv.response.success;
}

bool URSIProxy::movePtpPath(const rw::trajectory::QPath& q_path)
{
  caros_universalrobot::UrServiceMovePtpPath srv;
  for (unsigned int i = 0; i < q_path.size(); i++)
  {
    rw::math::Q q = q_path[i];
    rw::math::Q target(6, q[0], q[1], q[2], q[3], q[4], q[5]);
    double speed = q[6];
    double acceleration = q[7];
    double blend = q[8];
    verifyValueIsWithin(speed, SPEED_MIN, SPEED_MAX);
    verifyValueIsWithin(acceleration, ACCEL_MIN, ACCEL_MAX);
    verifyValueIsWithin(blend, BLEND_MIN, BLEND_MAX);
    srv.request.targets.push_back(caros::toRos(target));
    srv.request.speeds.push_back(caros::toRos(speed));
    srv.request.accelerations.push_back(caros::toRos(acceleration));
    srv.request.blends.push_back(caros::toRos(blend));
  }

  srv_move_ptp_path_.call<caros_universalrobot::UrServiceMovePtpPath>(srv);

  return srv.response.success;
}

bool URSIProxy::moveServoQ(const rw::math::Q& target, float time, float lookahead_time, float gain)
{
  verifyValueIsWithin(lookahead_time, SERVO_LOOKAHEAD_TIME_MIN, SERVO_LOOKAHEAD_TIME_MAX);
  verifyValueIsWithin(gain, SERVO_GAIN_MIN, SERVO_GAIN_MAX);
  caros_universalrobot::UrServiceServoQ srv;
  srv.request.target = caros::toRos(target);
  srv.request.time = caros::toRos(time);
  srv.request.lookahead_time = caros::toRos(lookahead_time);
  srv.request.gain = caros::toRos(gain);

  srv_move_servo_q_.call<caros_universalrobot::UrServiceServoQ>(srv);

  return srv.response.success;
}

bool URSIProxy::moveServoStop()
{
  caros_common_msgs::EmptySrv srv;
  srv_move_servo_stop_.call<caros_common_msgs::EmptySrv>(srv);

  return srv.response.success;
}

bool URSIProxy::moveForceModeStart(const rw::math::Transform3D<>& ref_t_offset, const rw::math::Q& selection,
                                   const rw::math::Wrench6D<>& wrench_target, int type, const rw::math::Q& limits)
{
  caros_universalrobot::UrServiceForceModeStart srv;
  srv.request.base2forceFrame = caros::toRos(ref_t_offset);
  std::vector<uint32_t> selection_vec;
  for (size_t i = 0; i < selection.size(); i++)
  {
    selection_vec.push_back(static_cast<uint32_t>(selection[i]));
  }

  srv.request.selection = selection_vec;
  srv.request.wrench = caros::toRos(wrench_target);
  srv.request.type = type;

  std::vector<float> limits_vec;
  for (size_t i = 0; i < limits.size(); i++)
  {
    limits_vec.push_back(static_cast<float>(limits[i]));
  }
  srv.request.limits = limits_vec;

  srv_move_force_mode_start_.call<caros_universalrobot::UrServiceForceModeStart>(srv);

  return srv.response.success;
}

bool URSIProxy::moveForceModeUpdate(const rw::math::Wrench6D<>& wrench_target)
{
  caros_universalrobot::UrServiceForceModeUpdate srv;
  srv.request.wrench = caros::toRos(wrench_target);
  srv_move_force_mode_update_.call<caros_universalrobot::UrServiceForceModeUpdate>(srv);

  return srv.response.success;
}

bool URSIProxy::moveForceModeStop()
{
  caros_common_msgs::EmptySrv srv;
  srv_move_force_mode_stop_.call<caros_common_msgs::EmptySrv>(srv);

  return srv.response.success;
}

bool URSIProxy::moveVelQ(const rw::math::Q& target, double acceleration, double time)
{
  caros_universalrobot::UrServiceMoveVelQ srv;
  srv.request.vel = caros::toRos(target);
  srv.request.acceleration = caros::toRos(acceleration);
  srv.request.time = caros::toRos(time);

  srv_move_vel_q_.call<caros_universalrobot::UrServiceMoveVelQ>(srv);

  return srv.response.success;
}

bool URSIProxy::moveVelT(const rw::math::VelocityScrew6D<>& target, double acceleration, double time)
{
  caros_universalrobot::UrServiceMoveVelT srv;
  srv.request.vel = caros::toRos(target);
  srv.request.acceleration = caros::toRos(acceleration);
  srv.request.time = caros::toRos(time);

  srv_move_vel_t_.call<caros_universalrobot::UrServiceMoveVelT>(srv);

  return srv.response.success;
}

bool URSIProxy::stop()
{
  caros_common_msgs::EmptySrv srv;

  srv_stop_.call<caros_common_msgs::EmptySrv>(srv);

  return srv.response.success;
}

/* Hardcoded since the connections are not added to a collection that can easily be iterated */
void URSIProxy::closePersistentConnections()
{
  srv_move_lin_.shutdown();
  srv_move_ptp_.shutdown();
  srv_move_ptp_path_.shutdown();
  srv_move_servo_q_.shutdown();
  srv_move_servo_stop_.shutdown();
  srv_move_force_mode_start_.shutdown();
  srv_move_force_mode_update_.shutdown();
  srv_move_force_mode_stop_.shutdown();
  srv_move_vel_q_.shutdown();
  srv_move_vel_t_.shutdown();
  srv_stop_.shutdown();
}

void URSIProxy::handleRobotState(const caros_control_msgs::RobotState& state)
{
  robot_state_ = state;
}

rw::math::Q URSIProxy::getQ()
{
  return caros::toRw(robot_state_.q);
}

rw::math::Q URSIProxy::getQd()
{
  return caros::toRw(robot_state_.dq);
}

rw::math::Wrench6D<> URSIProxy::getTCPForce()
{
  return caros::toRw(robot_state_.tcp_force);
}

rw::math::Transform3D<> URSIProxy::getTCPPose()
{
  return caros::toRw(robot_state_.tcp_pose);
}

bool URSIProxy::isEmergencyStopped()
{
  return robot_state_.e_stopped;
}

bool URSIProxy::isSecurityStopped()
{
  return robot_state_.s_stopped;
}

ros::Time URSIProxy::getTimeStamp()
{
  return robot_state_.header.stamp;
}
